#include "rsuctrl.h"
#include <QTime>
#include <QtEndian>

// 帧头
const static char *STX="\xFF\xFF";
// 帧尾
const static char *ETX="\xFF";
// 除去帧头、帧尾的最小帧长度：RSCTL(1) TYPE(1) BCC(1)
const static int MIN_FRAME_SIZE=3;
// 带帧头、帧尾的最小帧长度
const static int MIN_FRAME_SIZE_WITH_FRAME_HEAD_AND_END=6;

#define PRE_CHECK_FOR_RQ \
    if (QAbstractSocket::ConnectedState!=this->m_tcpSocket.state()) \
    { \
        return ERR_RSU_NOT_OPENED; \
    }

#define PRE_CHECK_FOR_RS PRE_CHECK_FOR_RQ


RsuCtrl::RsuCtrl(QObject *parent) : QObject(parent)
{
    memset(m_BeaconId, 0, 4);
    m_bstInterval=10;
    m_rsuPower=0;
    m_rsuChannel=0;
    m_nHeartBeatStyle=0;
    m_nHeartBeatInterval=1200*1000;
    m_status=DEV_STATUS_INIT;
    m_nNoResponseErrorCount=0;
    m_nExpectedFrameType=0;
    m_nBeaconChangeInterval=0;

    connect(&m_tcpSocket, &QTcpSocket::readyRead,
            this, &RsuCtrl::socketReadyRead);
    connect(&m_tcpSocket, &QAbstractSocket::stateChanged,
             this, &RsuCtrl::socketStateChanged);
    connect(&m_tcpSocket, SIGNAL(error(QAbstractSocket::SocketError)),
            this, SLOT(socketError(QAbstractSocket::SocketError)));

    connect(&m_timerDetectObu, &QTimer::timeout,
            this, &RsuCtrl::detectObuTimeout);
    m_pNoResponseTimer = new QTimer(this);
    // NOTE: 初始化指令、获取RSU信息指令耗时较长，需要设置较长的等待时间以免误判
    m_pNoResponseTimer->setInterval(10000);
    m_pNoResponseTimer->setSingleShot(true);
    connect(m_pNoResponseTimer, &QTimer::timeout,
            this, &RsuCtrl::handleNoResponseError);
}

RsuCtrl::~RsuCtrl()
{
}

void RsuCtrl::setId(int nId)
{
    m_nId = nId;
}
int RsuCtrl::getId()
{
    return m_nId;
}
void RsuCtrl::openRsu(const QString &host,quint16 port)
{
    m_sHost = host;
    m_nPort = port;
    m_tcpSocket.connectToHost(m_sHost,m_nPort);
}
void RsuCtrl::closeRsu()
{
    m_tcpSocket.close();
}
int RsuCtrl::initRsu(int nInterval, int nPower, int nChannel)
{
    m_bstInterval = getValueWithConfine(nInterval,1,10);
    m_rsuPower = getValueWithConfine(nPower,0,33);
    m_rsuChannel = (nChannel == 0)?0:1;
    return initRsuRq();
}

int RsuCtrl::getRsuInfo()
{
    PRE_CHECK_FOR_RQ;
    expectResult(0xE7);
    QByteArray data;
    data.append(0xE7);
    return wrapAndSendFrame(data);
}

int RsuCtrl::detectObu()
{
    if (DEV_STATUS_OK!=m_status)
    {
        // 没有初始化之前不检测OBU
        return -1;
    }

    quint8 App[16];
    memcpy(App, "\x07\x41\x87\x29\x20\x29\x00\x2B", 8);
    App[5]=ObuInfo::SYSFILE_SIZE;
    App[7]=43;   // 可能存在问题

    quint8 App_out[4+ObuInfo::SYSFILE_SIZE+51];  // 可能存在问题
    memset(App_out, 0, sizeof(App_out));
    quint8 ObuCfg[ObuInfo::OBU_CFG_SIZE+8];
    memset(ObuCfg, 0, sizeof(ObuCfg));

    quint8 Time[16];
    fillCurrentTime(Time);

    // NOTE: 在测试等情况下，修改BeanconId可以调整重新检测同一OBU的时间间隔
    modifyBeaconId();
    quint8 BeaconId[8];
    memcpy(BeaconId, m_BeaconId, 4);

    return initPrimRq((char *)BeaconId, (char *)Time,
            m_rsuChannel, 1, (char *)App, 0);
}

void RsuCtrl::tryReconnect()
{
    m_tcpSocket.close();
    if(QAbstractSocket::UnconnectedState == m_tcpSocket.state())
        m_tcpSocket.connectToHost(m_sHost,m_nPort);
}

void RsuCtrl::checkRsuStatus()
{
    if (DEV_STATUS_OK!=m_status)
    {
        // 仅在设备状态正常时有必要检测设备状态
        return;
    }

    qint64 currentTime=QDateTime::currentMSecsSinceEpoch();
    if (qAbs(currentTime-m_nDevOKTime)>m_nHeartBeatInterval)
    {
        // 如果近期通讯正常，没必要专门发送指令检测
        qDebug()<<tr("rsu[%1] heartbeat check style[%2]")
                       .arg(getId())
                       .arg(m_nHeartBeatStyle);
        // 一些型号的天线不支持获取状态，只能以初始化作为心跳检测手段
        switch(m_nHeartBeatStyle)
        {
        case 0:
            getRsuInfo();
            break;
        case 1:
            initRsu(m_bstInterval,m_rsuPower,m_rsuChannel);
            break;
        case 2:
            tryReconnect();
            break;
        default:
            break;
        }
    }
}

void RsuCtrl::setHeartBeat(int nStyle,int nInterval)
{
    m_nHeartBeatStyle=nStyle;
    m_nHeartBeatInterval=getValueWithConfine(nInterval, 10*1000, 0x7FFFFFFF);
}

void RsuCtrl::startDetectObu(int nInterval)
{
    m_timerDetectObu.setInterval(nInterval);
    m_timerDetectObu.start();
}
void RsuCtrl::stopDetectObu()
{
    m_timerDetectObu.stop();
}

int RsuCtrl::releaseObu()
{
    int id = 1;
    int type = 0;
    PRE_CHECK_FOR_RQ;
    QByteArray data;
    data.append(0xF3);
    // 含义未知
    data.append("\x02\xCA\x21\x2F\x40\x03\x91\x60", 8);
    data.append(quint8(id));
    data.append(quint8(type));
//    data.append(0x01);
//    data.append(0x00);
    return wrapAndSendFrame(data);
}
// 该值搭配 releaseobu 函数使用 也不好使
void RsuCtrl::setBeaconChangeInterval(int nInterval)
{
    m_nBeaconChangeInterval=nInterval;
}

int RsuCtrl::initRsuRq()
{
    expectResult(0xE0);
    quint8 Time[16];
    fillCurrentTime(Time);
    PRE_CHECK_FOR_RQ;
    QByteArray data;
    data.append(0xF0);
    data.append((char*)Time,4);
    data.append(quint8(m_bstInterval));
    data.append(quint8(m_rsuPower));
    data.append(quint8(m_rsuChannel));
    return wrapAndSendFrame(data);
}

int RsuCtrl::initPrimRq(char *BeaconID, char *Time, int Profile, int MandApplicationlist, char *MandApplication, int Profilelist)
{
    PRE_CHECK_FOR_RQ;
    QByteArray data;
    data.append(0xF1);
    data.append("\xFF\xFF\xFF\xFF", 4);
    // 含义未知
    data.append("\x50\x03", 2);
    // PDU号码，不得设定到 0000 或 0001
    quint8 pdu=0x02;
    // 分段字头，在pdu=0x02时，应该为0x91
    quint8 head = 0x81 | (pdu<<3);
    data.append(head);
    // INITIALIZATION.request，按GBT-20851.3-2007附录B
    data.append(0xC0);
    data.append(BeaconID, 4);
    data.append(Time, 4);
    data.append(quint8(Profile));
    data.append(quint8(MandApplicationlist));
    // 不知道为何要去掉本条命令的长度（只有一条命令）
    data.append(MandApplication+1, getCmdGroupLen(MandApplicationlist, MandApplication)-1);
    data.append(quint8(Profilelist));
    return wrapAndSendFrame(data);
}

void RsuCtrl::wrapFrame(QByteArray &data)
{
    data.replace("\xFE", 1, "\xFE\x00", 2);
    data.replace("\xFF", 1, "\xFE\x01", 2);
    quint8 rsctl = getRSCTL();
    data.prepend(rsctl);
    quint8 bcc = getXOR(data);
    data.append(bcc);
    data.prepend(STX);
    data.append(ETX);
}

int RsuCtrl::wrapAndSendFrame(QByteArray &data)
{
    if (char(0xF1)!=data[0])
    {
        qDebug()<<tr("<%1:%2>: command[%3]").arg(m_sHost).arg(m_nPort)
                        .arg(data.toHex().data());
    }
    wrapFrame(data);

    qint64 nWrittenDataLen=m_tcpSocket.write(data);
    if (nWrittenDataLen!=data.length())
    {
        // Note: 20151221使用成谷天线控制盒测试发现：
        //  如果天线电源被关闭，write仍返回成功，直到约3分钟后才有
        // socketStateChanged等事件，所以这个判断条件基本不会被走到！
        qDebug()<<"m_tcpSocket.write failure";
        return ERR_RSU_OTHER;
    }
    return ERR_SUCCESS;
}
int RsuCtrl::getValueWithConfine(int n, int nMin, int nMax)
{
    if (n<nMin) n=nMin;
    if (n>nMax) n=nMax;
    return n;
}

void RsuCtrl::setStatus(const int newStatus)
{
    if (newStatus!=m_status)
    {
        m_status = newStatus;
        emit statusChanged(newStatus);
    }
    if (DEV_STATUS_OK==newStatus)
    {
        m_nDevOKTime = QDateTime::currentMSecsSinceEpoch();
        m_nNoResponseErrorCount = 0;
    }
}

void RsuCtrl::fillCurrentTime(quint8 *Time)
{
    quint32 uTime = QDateTime::currentDateTime().toTime_t();
    uTime=qToBigEndian(uTime);
    memcpy(Time, &uTime, 4);
}

int RsuCtrl::getCmdGroupLen(int cmdCount, const char *cmdGroup)
{
    int n=0;
    for (int i=0; i<cmdCount; i++)
    {
        n+=(cmdGroup[n]+1);
    }
    return n;
}
quint8 RsuCtrl::getRSCTL()
{
    if (m_frameIndex>7)
    {
       m_frameIndex=0;
    }
    quint8 n=0x80+m_frameIndex;
    m_frameIndex++;
    return n;
}
quint8 RsuCtrl::getXOR(const QByteArray &data)
{
    quint8 n=0x00;
    for(int it=0; it<data.length(); ++it)
    {
        n ^= data[it];
    }
    return n;
}

void RsuCtrl::expectResult(int nFrameType)
{
    m_nExpectedFrameType=nFrameType;
    m_pNoResponseTimer->start();
}



void RsuCtrl::socketError(QAbstractSocket::SocketError socketError)
{
    qDebug()<<tr("<%1:%2>: Socket错误[%3][%4]")
                    .arg(m_sHost)
                    .arg(m_nPort)
                    .arg(socketError)
                    .arg(this->m_tcpSocket.errorString());
    QTimer::singleShot(10000, this, &RsuCtrl::tryReconnect);

}
void RsuCtrl::socketStateChanged(QAbstractSocket::SocketState socketState)
{
    if (QAbstractSocket::ConnectedState==socketState)
    {
        initRsu(m_bstInterval,m_rsuPower,m_rsuChannel);
    }
    else
    {
        setStatus(DEV_STATUS_ERR);
    }
}
void RsuCtrl::socketReadyRead()
{
    QByteArray dataRead = m_tcpSocket.readAll();
    if(dataRead.length() == 0)
    {
        qDebug()<<tr("<%1:%2>: 0 byte read, disconnect.").arg(m_sHost)
                       .arg(m_nPort);
        m_tcpSocket.disconnectFromHost();
        return;
    }
    processDataRead(dataRead,NULL,0);
}
bool RsuCtrl::processDataRead(const QByteArray &dataRead, QByteArray *pExpectedFrame, quint8 nExpectedFrameType)
{
    m_dataRead.append(dataRead);
    while (m_dataRead.length()>MIN_FRAME_SIZE_WITH_FRAME_HEAD_AND_END)
    {
        int start = m_dataRead.indexOf(STX, 0);
        if (start<0)
        {
            // 没有包头，丢弃所有数据
            qDebug()<<tr("<%1:%2>: no frame head, discard %3 bytes.")
                            .arg(m_sHost)
                            .arg(m_nPort)
                            .arg(m_dataRead.length());
            m_dataRead.clear();
            return false;
        }
        int end = m_dataRead.indexOf(ETX, strlen(STX));
        if (end<0)
        {
            // 没有包尾，等待再次接收数据
            return false;
        }
        // 抽取去除包头、包尾的帧数据
        QByteArray frame = m_dataRead.mid(start+strlen(STX), end-start-strlen(STX));
        // 从读入缓存中去除本帧数据（连同包头、包尾）
        m_dataRead.remove(0, end+strlen(ETX));
        processRawFrame(frame, pExpectedFrame, nExpectedFrameType);
    }
}
bool RsuCtrl::processRawFrame(QByteArray &frame, QByteArray *pExpectedFrame, quint8 nExpectedFrameType)
{
    Q_UNUSED(pExpectedFrame)
    Q_UNUSED(nExpectedFrameType)
    if (frame.length()<MIN_FRAME_SIZE)
    {
        // 长度不足，丢弃帧数据
        qDebug()<<tr("<%1:%2>: too short, discard frame[%3]")
                        .arg(m_sHost)
                        .arg(m_nPort)
                        .arg(frame.toHex().data());
        return false;
    }
    // BCC校验
    quint8 bcc = getXOR(frame);
    if (0!=bcc)
    {
        // BCC校验失败，丢弃帧数据
        qDebug()<<tr("<%1:%2>: bcc error, discard frame[%3]").arg(m_sHost).arg(m_nPort)
                        .arg(frame.toHex().data());
        return false;
    }
    frame.replace("\xFE\x01", 2, "\xFF", 1);
    frame.replace("\xFE\x00", 2, "\xFE", 1);

    qDebug()<<tr("<%1:%2>: frame[%3]").arg(m_sHost).arg(m_nPort)
                    .arg(frame.toHex().data());
   quint8 nFrameIndex=frame[0];
    quint8 nFrameType=frame[1];
    // 去除帧序号和帧类型
    frame.remove(0, 2);
    // 去除BCC
    frame.remove(frame.length()-1, 1);
//    if (NULL!=pExpectedFrame)
//    {
//        bool bRlt=(nFrameType==nExpectedFrameType);
//        if (bRlt)
//        {
//            pExpectedFrame->clear();
//            pExpectedFrame->append(frame);
//        }
//        return bRlt;
//    }
//    else
//    {
        emit newFrameArrived(nFrameIndex, nFrameType, frame);
        processRsuFrame(nFrameIndex, nFrameType, frame);
        return false;
//    }
}
void RsuCtrl::processRsuFrame(quint8 nFrameIndex, quint8 nFrameType, const QByteArray &frame)
{
    Q_UNUSED(nFrameIndex);

    if (nFrameType==m_nExpectedFrameType)
    {
        this->m_pNoResponseTimer->stop();
        m_nExpectedFrameType=0;
    }

    if (0xE0==nFrameType)
    {
        processRsuInitFrame(frame);
    }
    else if (0xE1==nFrameType)
    {
        processObuInitFrame(frame);
    }
    else if (0xE7==nFrameType)
    {
        processRsuInfoFrame(frame);
    }
}
void RsuCtrl::processRsuInitFrame(const QByteArray &frame)
{
    // RSUStatus[1]+rlen[1]+RSUinfo(4..?)，至少6字节
    if (frame.length()<6)
    {
        qDebug()<<tr("error rsu init frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }
    int nRsuStatus=frame[0];
    int nRsuInfoLen=frame[1];
    if ((nRsuInfoLen<4) ||
        (frame.length()<(nRsuInfoLen+2)))
    {
        qDebug()<<tr("error rsu init frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }
    QByteArray baRsuInfo=frame.mid(2, nRsuInfoLen);
    memcpy(m_BeaconId, baRsuInfo.data(), 4);
    setStatus(DEV_STATUS_OK);
    emit rsuInited(nRsuStatus, baRsuInfo);
}

/*!
 * \brief 处理Rsu信息帧
 * \param 因为搜林和成谷的返回格式略有差异，在处理时，假定成谷的信息帧中的RSUStatus=0
 */
void RsuCtrl::processRsuInfoFrame(const QByteArray &frame)
{
    // 搜林：RSUStatus[1]+rlen[1]+RSUinfo(4..?)，至少6字节
    // 成谷：rlen[1]+RSUinfo(4..?)，至少5字节
    int nIndexForDataLen = 0;

    if ((frame.length()-nIndexForDataLen)<5)
    {
        qDebug()<<tr("error rsu info frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }
    int nRsuInfoLen=frame[nIndexForDataLen];
    if ((nRsuInfoLen<4) ||
        (frame.length()<(nRsuInfoLen+nIndexForDataLen+1)))
    {
        qDebug()<<tr("error rsu info frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }
    QByteArray baRsuInfo=frame.mid(nIndexForDataLen+1, nRsuInfoLen);
    // memcpy(m_BeaconId, baRsuInfo.data(), 4);
    setStatus(DEV_STATUS_OK);
    emit rsuInfoGot(baRsuInfo);
}

void RsuCtrl::processObuInitFrame(const QByteArray &frame)
{
    int index=0;
    int nReturnStatus=frame[index];
    index++;
    if (0!=nReturnStatus)
    {
        qDebug()<<tr("error obu return status. frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }
        index+=4;   // mac
        index+=2;   // 神秘的c0 03
    index++;    // 91-head
    index++;    // d0-INITIALIZATION.response

    // 进一步判断返回长度
    const int nObuCfgLen=7;
    if (frame.length()<index+2+nObuCfgLen)
    {
        qDebug()<<tr("error obu frame length. frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }

    int nProfile=frame[index];
    index++;
    int nAppList=frame[index];
    index++;
    int nAppLen=frame.length()-index-nObuCfgLen;
    if (nAppLen<1)
    {
        qDebug()<<tr("error obu app length. frame[%1]")
                        .arg(frame.toHex().data());
        return;
    }
    QByteArray baApp=frame.mid(index, nAppLen);
    QByteArray baObuCfg=frame.right(nObuCfgLen);
    processObuInfo(nProfile, nAppList, baApp, baObuCfg);
}

void RsuCtrl::processObuInfo(int nProfile, int nAppList, const QByteArray &baApp, const QByteArray &baObuCfg)
{
    if ((0!=nProfile) && (1!=nProfile))
    {
        // profile应该同搜寻OBU时指定的信道号一致
        qDebug()<<tr("unexpected profile[%1] value!").arg(nProfile);
    }
    if (1!=nAppList)
    {
        qDebug()<<tr("nAppList[%1]!=1.").arg(nAppList);
    }

    if (baApp.length()<4+ObuInfo::SYSFILE_SIZE)
    {
        qDebug()<<tr("error appLen[%1], app[%2].")
                       .arg(baApp.length())
                       .arg(baApp.toHex().data());
        return;
    }
    ObuInfo newObu;
    bool bRlt=newObu.setObuInfo((const quint8 *)baApp.data()+4, ObuInfo::SYSFILE_SIZE, (const quint8 *)baObuCfg.data());
    if (!bRlt)
    {
        qDebug()<<tr("trans obu info error, app[%1].")
                       .arg(baApp.toHex().data());
        return;
    }
    setStatus(DEV_STATUS_OK);
    emit obuDetected(newObu);
}
void RsuCtrl::detectObuTimeout()
{
    detectObu();
}

void RsuCtrl::handleNoResponseError()
{
    if (0xE0==m_nExpectedFrameType)
    {
        // 超时重发初始化指令
        initRsu(m_bstInterval,m_rsuPower,m_rsuChannel);
    }
    else if (0xE7==m_nExpectedFrameType)
    {
        // 超时重发获取RSU信息指令
        getRsuInfo();
    }

    m_nNoResponseErrorCount++;
    if (m_nNoResponseErrorCount>2)
    {
        setStatus(DEV_STATUS_ERR);
    }

    if (m_nNoResponseErrorCount>5)
    {
        tryReconnect();
    }
}

void RsuCtrl::modifyBeaconId()
{
    if (0==m_nBeaconChangeInterval)
    {
        return;
    }
    static quint64 nLastCheck=0;
    quint64 nCurrentCheck=QDateTime::currentMSecsSinceEpoch()/m_nBeaconChangeInterval;
    if (nLastCheck!=nCurrentCheck)
    {
        nLastCheck=nCurrentCheck;
        m_BeaconId[0]=m_BeaconId[0]+1;
    }
}

